#include <cmath>
#include <ros/ros.h>
#include "ares_pathing/path.h"

//Default Constructor
Path::Path(cell c){ //start with what will be the last cell of the path
    route.push_back(c);
}

/*Preconditions: the x & y coordinates of a cell to be added to the path
  Postconditions: A representation of the cell is added to the front of the path
*/
bool Path::addCell(int x, int y){
    cell c;
    c.xCell = x;
    c.yCell = y;
    route.insert(route.begin(),1,c);
    return true;
}

void Path::setStart(cell c){
    //put in the start of the path    
    route.insert(route.begin(),1,c);
}

void Path::setRealGoal(cell g){
    route.push_back(g);
}

/*Preconditions: the ideal goal of the path
  Postconditions: all of the cells have headings assigned, and the path score is saved and returned
*/
float Path::calcScore(cell g){
    //start the score using the general length of the path
    score = route.size();
    //define the headings for each cell, and add total changes to the score    
    for(int i=1;i<route.size();i++){
        route[i].heading = atan2((route[i].yCell-route[i-1].yCell),(route[i].xCell-route[i-1].xCell));
        score+=route[i].heading-route[i-1].heading;
    }
    //add 5 if ending at unknown instead of known
    if(route.back().xCell != g.xCell || route.back().yCell != g.yCell)
        score+=5;
    ROS_INFO("Path Score: %f",score);
    return score;
}

/*Preconditions: none
  Postconditions: all the points in the path have a velocity set, and the next few points are returned in a message format
*/
void Path::setVel(){
    //set the goal velocity to be zero.
    route.back().vel = 0;
    //fill in the accel region
    for(int i=1;i<accSteps;i++){
        route[i].vel = route.front().vel+i/accSteps*(maxVel-route.front().vel);//account for the bot already having a velocity
    }
    //fill in the const. velocity region
    for(int i=accSteps;i<(route.size()-decSteps);i++){
        route[i].vel = maxVel;
    }
    //fill in the decel region
    for(int i=(route.size()-decSteps);i<route.size();i++){
        route[i].vel = (route.size()-i-1)/decSteps*maxVel;
    }
}

/*Preconditions: none
  Postconditions: the next 5 path points are generated, including the skip factor. If you reach the end, the last point is just repeated.
*/
void Path::pathPointGen(int s,path_point* p){
    ROS_INFO("Creating path point generation");
    int sz = 0;    
    for(int i=1;i<5*s;i+=s){        
        if(i>=route.size()){
            p[sz].vel = route.back().vel;
            p[sz].globalrad = route.back().heading;
            p[sz].deltarad = route.back().heading-route[route.size()-2].heading;
        }   
        else{        
            p[sz].vel = route[i].vel;
            p[sz].globalrad = route[i].heading;
            p[sz].deltarad = route[i].heading-route[i-1].heading;
        }
        sz++;   
    }
    ROS_INFO("p populated");
}


